#pragma once

#ifndef _IMU_HANDLE_H_
#define _IMU_HANDLE_H_

#include"utility.h"

namespace vld_imu_slam{

using gtsam::symbol_shorthand::X; // Pose3 (x,y,z,r,p,y)
using gtsam::symbol_shorthand::V; // Vel   (xdot,ydot,zdot)
using gtsam::symbol_shorthand::B; // Bias  (ax,ay,az,gx,gy,gz)

class IMUHandle{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    typedef shared_ptr<IMUHandle> Ptr;

    Config::Ptr p_config_;

    deque<sensor_msgs::Imu> imu_deq_opt_;
    deque<sensor_msgs::Imu> imu_deq_integ_;

    IMUHandle();
    IMUHandle(Config::Ptr p_config){
        p_config_ = p_config;

        // imu预积分的噪声协方差
        boost::shared_ptr<gtsam::PreintegrationParams> p = gtsam::PreintegrationParams::MakeSharedU(p_config_->imu_gravity_);
        p->accelerometerCovariance  = gtsam::Matrix33::Identity(3,3) * pow(p_config_->imu_acc_noise_std_, 2); // acc white noise in continuous
        p->gyroscopeCovariance      = gtsam::Matrix33::Identity(3,3) * pow(p_config_->imu_gyr_noise_std_, 2); // gyro white noise in continuous
        p->integrationCovariance    = gtsam::Matrix33::Identity(3,3) * pow(1e-4, 2); // error committed in integrating position from velocities
        gtsam::imuBias::ConstantBias prior_imu_bias((gtsam::Vector(6) << 0, 0, 0, 0, 0, 0).finished());; // assume zero initial bias
        imuIntegrator_ = new gtsam::PreintegratedImuMeasurements(p, prior_imu_bias); 

        // 噪声先验
        priorPoseNoise_  = gtsam::noiseModel::Diagonal::Sigmas((gtsam::Vector(6) << 1e-2, 1e-2, 1e-2, 1e-2, 1e-2, 1e-2).finished()); // rad,rad,rad,m, m, m
        priorVelNoise_   = gtsam::noiseModel::Isotropic::Sigma(3, 1e4); // m/s
        priorBiasNoise_  = gtsam::noiseModel::Isotropic::Sigma(6, 1e-3); // 1e-2 ~ 1e-3 seems to be good
        // 激光里程计scan-to-map优化过程中发生退化，则选择一个较大的协方差
        correctionNoise_ = gtsam::noiseModel::Diagonal::Sigmas((gtsam::Vector(6) << 0.05, 0.05, 0.05, 0.1, 0.1, 0.1).finished()); // rad,rad,rad,m, m, m
        correctionNoise2_ = gtsam::noiseModel::Diagonal::Sigmas((gtsam::Vector(6) << 1, 1, 1, 1, 1, 1).finished()); // rad,rad,rad,m, m, m
        noiseModelBetweenBias_ = (gtsam::Vector(6) << p_config_->imu_acc_bias_n_, 
                                                                        p_config_->imu_acc_bias_n_, p_config_->imu_acc_bias_n_, 
                                                                        p_config_->imu_gyr_bias_n_, p_config_->imu_gyr_bias_n_, p_config_->imu_gyr_bias_n_).finished();

        imuIntegratorOpt_ = new gtsam::PreintegratedImuMeasurements(p, prior_imu_bias); 

        is_get_framestamp_.store(false);
        is_set_frametime_integ_.store(false);
    }
    deque<nav_msgs::Odometry> imuIntegration();

    void pushbackIMUData(sensor_msgs::Imu imu_data_converted);

    void setFrameTimestamp(const double time,const SE3& pose);

    void imuPreintegration();

    void setFrameTimestampInteg(const double time);

private:
    mutex imu_integ_mutex_,imu_opt_mutex_,imuIntegrator_mutex_,imuInteg_thread_mutex_;

    bool doneFirstOpt_ = false;
    double last_imu_timestamp_ = -1;
    gtsam::PreintegratedImuMeasurements *imuIntegrator_;
    gtsam::NavState prev_state_odom_;
    gtsam::imuBias::ConstantBias prev_bias_odom_;
    condition_variable imu_integ_cv_;
    atomic<bool> is_set_frametime_integ_;
    double integ_imu_cur_timestamp_ = -1;

    condition_variable imu_opt_cv_;
    SE3 pose_current_;
    mutex imu_preinteg_mutex_;
    bool systemInitialized_ = false;
    // ISAM2优化器
    gtsam::ISAM2 isam2_;
    gtsam::NonlinearFactorGraph graphFactors_;
    gtsam::Values graphValues_;
    // 噪声协方差
    gtsam::noiseModel::Diagonal::shared_ptr priorPoseNoise_;
    gtsam::noiseModel::Diagonal::shared_ptr priorVelNoise_;
    gtsam::noiseModel::Diagonal::shared_ptr priorBiasNoise_;
    gtsam::noiseModel::Diagonal::shared_ptr correctionNoise_;
    gtsam::noiseModel::Diagonal::shared_ptr correctionNoise2_;
    gtsam::Vector noiseModelBetweenBias_;
    // imu因子图优化过程中的状态变量
    gtsam::Pose3 prev_pose_opt_;
    gtsam::Vector3 prev_vel_opt_;
    gtsam::NavState prev_state_opt_;
    gtsam::imuBias::ConstantBias prev_bias_opt_;
    gtsam::PreintegratedImuMeasurements *imuIntegratorOpt_;
    int frame_counter_ = 0;
    double frame_timestamp_cur_ = -1;
    double last_imu_timestamp_opt_ = -1;
    atomic<bool> is_get_framestamp_;

    void popUsenessData(deque<sensor_msgs::Imu>& imu_deq,double time);

    void resetOptimization();

    bool failureDetection(const gtsam::Vector3& velCur, const gtsam::imuBias::ConstantBias& biasCur);

    void resetParams();

};



}
#endif  
